using System;

namespace Diversity.Robotics.Navigation
{
    /// <summary>
    /// A velocity representation containing translational velocity and rotational velocity.
    /// </summary>
    public class Velocity : ICloneable
    {
        private double _translation;
        private double _rotation;

        /// <summary>
        /// Creates a velocity representation containing translational velocity and rotational velocity.
        /// </summary>
        /// <param name="translation">The translation.</param>
        /// <param name="rotation">The rotation.</param>
        public Velocity(double translation, double rotation)
        {
            _translation = translation;
            _rotation = rotation;
        }

        /// <summary>
        /// The translational velocity.
        /// </summary>
        public double Translation
        {
            get { return _translation; }
            set { _translation = value; }
        }

        /// <summary>
        /// The rotational velocity.
        /// </summary>
        public double Rotation
        {
            get { return _rotation; }
            set { _rotation = value; }
        }

        /// <summary>
        /// Creates a string representation.
        /// </summary>
        /// <returns></returns>
        public override string ToString()
        {
            return "Velocity: (v: " + _translation + " ,w: " + _rotation + ")";
        }

        /// <summary>
        /// Creates a copy of this velocity.
        /// </summary>
        /// <returns></returns>
        public object Clone()
        {
            return new Velocity(_translation, _rotation);
        }

        /// <summary>
        /// Adds a rotational component to the current rotation.
        /// </summary>
        /// <param name="rotation"></param>
        public void Rotate(double rotation)
        {
            _rotation = _rotation + rotation;
            CleanRotation();
        }

        /// <summary>
        /// Adds a rotation within a limit. Useful for when applying an
        /// acceleration within a max turn speed.
        /// </summary>
        /// <param name="rotation"></param>
        /// <param name="limit"></param>
        public void RotateWithinLimit(double rotation, double limit)
        {
            _rotation = _rotation + rotation;
            CleanRotation();
            if (_rotation > limit)
            {
                _rotation = limit;
            } else if (_rotation < -limit)
            {
                _rotation = -limit;
            }
        }

        /// <summary>
        /// Dampens the rotational component towards zero.
        /// </summary>
        /// <param name="amount"></param>
        public void Dampen(double amount)
        {
            if (_rotation != 0)
            {
                if (_rotation < 0)
                {
                    _rotation += amount;
                    if (_rotation > 0)
                    {
                        _rotation = 0;
                    }
                } else if (_rotation > 0)
                {
                    _rotation -= amount;
                    if (_rotation < 0)
                    {
                        _rotation = 0;
                    }
                }
            }
        }

        /// <summary>
        /// Fixes angles to a +/- Math.PI range
        /// </summary>
        public void CleanRotation()
        {
            if (_rotation > Math.PI)
            {
                _rotation -= Math.PI*2;
            } else if (_rotation < -Math.PI)
            {
                _rotation += Math.PI*2;
            }
        }

        /// <summary>
        /// Adds two velocities together
        /// </summary>
        /// <param name="vel1"></param>
        /// <param name="vel2"></param>
        /// <returns></returns>
        public Velocity Add(Velocity vel1, Velocity vel2)
        {
            return new Velocity(vel1.Translation + vel2.Translation, vel1.Rotation + vel2.Rotation);
        }

        /// <summary>
        /// Multiplies a velocity by 2*2 matrix
        /// </summary>
        /// <param name="vel"></param>
        /// <param name="matrix"></param>
        /// <returns></returns>
        public Velocity Multiply(Velocity vel, double[,] matrix)
        {
            // assumed to be a 2*2 matrix
            if (matrix.Rank == 2 && matrix.GetLength(0) == 2 && matrix.GetLength(1) == 2)
            {
                return new Velocity(vel.Translation * matrix[0, 0] + vel.Rotation * matrix[0, 1],
                    vel.Translation * matrix[1, 0] + vel.Rotation * matrix[1, 1]);
            }
            else
            {
                throw new Exception("Invalid Argument Exception. Matrix is not 2*2");
            }
        }
    }
}
